#include <rtthread.h>
#include <rtdevice.h>
#include "can_common.h"
#include "DJI/dji.h"
#include "board_com.h"
#include "damiao.h"
#include "dm_imu.h"
#include "ringbuffer.h"

#define CAN1_DEV_NAME       "can1"      /* CAN 设备名称 */
#define CAN2_DEV_NAME       "can2"

#define LOG_TAG              "can"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

static rt_sem_t rx1_sem;     /* 用于接收消息的信号量 */ 
static rt_sem_t rx2_sem;


rt_device_t can1_bus;            /* CAN 设备句柄 */
rt_device_t can2_bus;

static rt_atomic_t atomic_can1_send_lock = 0;
static rt_atomic_t atomic_can2_send_lock = 0;

static generic_ring_buffer_t *can1_rx_buffer = RT_NULL;
static generic_ring_buffer_t *can2_rx_buffer = RT_NULL;

/* 接收数据回调函数 */
rt_inline rt_err_t can_rx1_call(rt_device_t dev, rt_size_t size)
{
    if (rx1_sem != RT_NULL)
    {
        static struct rt_can_msg temp_msg = {0}; // 创建临时变量
        temp_msg.hdr_index = -1;
        
        rt_device_read(can1_bus, 0, &temp_msg, sizeof(temp_msg));
        
        /* 存入环形缓冲区 */
        generic_ring_buffer_put(can1_rx_buffer, &temp_msg);
        rt_sem_release(rx1_sem); // 入队后触发信号量
    }
    return RT_EOK;
}

rt_inline rt_err_t can_rx2_call(rt_device_t dev, rt_size_t size)
{
    if (rx2_sem != RT_NULL)
    {
        static struct rt_can_msg temp_msg = {0}; // 创建临时变量
        temp_msg.hdr_index = -1;
        
        rt_device_read(can2_bus, 0, &temp_msg, sizeof(temp_msg));
        
        /* 存入环形缓冲区 */
        generic_ring_buffer_put(can2_rx_buffer, &temp_msg);
        rt_sem_release(rx2_sem); // 成功入队后触发信号量
    }

    return RT_EOK;
}

static void can_rx1_thread(void *parameter)
{
    static struct rt_can_msg msg;
    while (1)
    {
        if(rt_sem_take(rx1_sem, RT_WAITING_FOREVER)==RT_EOK){
            generic_ring_buffer_get(can1_rx_buffer, &msg);
            DecodeDJIMotor(msg,can1_bus);
            DMMotorDecode(msg);
        }
        rt_thread_mdelay(1);
    }
}

static void can_rx2_thread(void *parameter)
{
    static struct rt_can_msg msg;
    while (1)
    {
        if (rt_sem_take(rx2_sem, RT_WAITING_FOREVER)==RT_EOK)
        {
            generic_ring_buffer_get(can2_rx_buffer, &msg);
            DecodeDJIMotor(msg,can2_bus);
            board_recv(msg);
            IMU_UpdateData(&msg);
        }
        rt_thread_mdelay(1);
    }
}

void can_send(rt_device_t can_bus, struct rt_can_msg *msg)
{
    if (can_bus == can1_bus)
    {
        if (rt_atomic_flag_test_and_set(&atomic_can1_send_lock) == 0)
        {
            rt_device_write(can1_bus, 0, msg, sizeof(struct rt_can_msg));
            rt_atomic_flag_clear(&atomic_can1_send_lock);
        }
    }
    else if (can_bus == can2_bus)
    {
        if (rt_atomic_flag_test_and_set(&atomic_can2_send_lock) == 0)
        {
            rt_device_write(can2_bus, 0, msg, sizeof(struct rt_can_msg));
            rt_atomic_flag_clear(&atomic_can2_send_lock);
        }
    }
}


int can_init(void)
{
    rt_err_t res;
    rt_size_t  size;
    rt_thread_t thread;


   /* 创建一个动态信号量，初始值是 0 */
    rx1_sem = rt_sem_create("rx1_sem", 0, RT_IPC_FLAG_PRIO);
    if (rx1_sem == RT_NULL)
    {
       LOG_E("create dynamic semaphore failed.\n");
       res =RT_ERROR;
    }
    rx2_sem = rt_sem_create("rx2_sem", 0, RT_IPC_FLAG_PRIO);
    if (rx2_sem == RT_NULL)
    {
       LOG_E("create dynamic semaphore failed.\n");
       res =RT_ERROR;
    }

    can1_rx_buffer = generic_ring_buffer_create(16, sizeof(struct rt_can_msg));
    can2_rx_buffer = generic_ring_buffer_create(16, sizeof(struct rt_can_msg));

    /* 查找 CAN 设备 */
    can1_bus = rt_device_find(CAN1_DEV_NAME);
    if (!can1_bus)
    {
        LOG_E("CAN device %s not found!",CAN1_DEV_NAME);
    }
    can2_bus = rt_device_find(CAN2_DEV_NAME);
    if (!can2_bus)
    {
        LOG_E("CAN device %s not found!",CAN1_DEV_NAME);
    }

 
    struct rt_can_filter_item items[1] =
    {
        RT_CAN_FILTER_ITEM_INIT(0x00, 0, 0, 0, 0x00),
        /* 不过滤，hdr 为 - 1，设置默认过滤表 */
    };
    struct rt_can_filter_config cfg = {1, 1, items}; /* 一共有 1 个过滤表 */

    /* 设置 CAN 通信的波特率为 1000kbit/s*/
    res = rt_device_control(can1_bus, RT_CAN_CMD_SET_BAUD, (void *)CAN1MBaud);
    /* 设置 CAN 的工作模式为正常工作模式 */
    res = rt_device_control(can1_bus, RT_CAN_CMD_SET_MODE, (void *)RT_CAN_MODE_NORMAL);
    /* 以中断接收及发送方式打开 CAN 设备 */
    res = rt_device_open(can1_bus, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);  
    /* 设置硬件过滤表 */
    res = rt_device_control(can1_bus, RT_CAN_CMD_SET_FILTER, &cfg);
    /* 设置接收回调函数 */
    res = rt_device_set_rx_indicate(can1_bus, can_rx1_call);
    /* 开启can*/
    res = rt_device_control(can1_bus,RT_CAN_CMD_START,(void *)1);


    res = rt_device_open(can2_bus, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
    /* 设置 CAN 通信的波特率为 1000kbit/s*/
    res = rt_device_control(can2_bus, RT_CAN_CMD_SET_BAUD, (void *)CAN1MBaud);
    /* 设置 CAN 的工作模式为正常工作模式 */
    res = rt_device_control(can2_bus, RT_CAN_CMD_SET_MODE, (void *)RT_CAN_MODE_NORMAL);    
    /* 设置硬件过滤表 */
    res = rt_device_control(can2_bus, RT_CAN_CMD_SET_FILTER, &cfg);
    /* 设置接收回调函数 */                                                     
    res = rt_device_set_rx_indicate(can2_bus, can_rx2_call);
    /* 开启can*/
    res = rt_device_control(can2_bus,RT_CAN_CMD_START,(void *)1);

    RT_ASSERT(res == RT_EOK);

    /* 创建数据接收线程 */
    thread = rt_thread_create("can_rx1", can_rx1_thread, RT_NULL, 2048, 13, 10);
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        LOG_E("create can_rx1 thread failed!\n");
    }

    thread = rt_thread_create("can_rx2", can_rx2_thread, RT_NULL, 2048, 13, 10);
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        LOG_E("create can_rx2 thread failed!\n");
    }

    return RT_EOK;
}
/* 导出到 msh 命令列表中 */
INIT_ENV_EXPORT(can_init);
